#!/usr/bin/env python

import rospy
import tf2_ros
from tf.transformations import quaternion_from_euler,euler_matrix
from ros_numpy import numpify,msgify
import numpy as np

class ActiveSlotNode:
    def __init__(self):
        # Initialize the node
        rospy.init_node('active_slot_node')

        # Create a TransformListener
        self.tf_buffer = tf2_ros.Buffer()
        self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)

        self.rate=rospy.Rate(50)
        
        
        self.T_T_V=np.eye(4)
        self.T_B_V=np.eye(4)
        self.T_T_B=np.eye(4)
        self.T_C_B=np.eye(4)
        
        self.run()
        
    def run(self):
        while not rospy.is_shutdown():
            try:
                # Lookup the transform from 'vrpn' to 'target'
                transform = self.tf_buffer.lookup_transform( 'vrpn','target', rospy.Time(0), rospy.Duration(1.0))
                self.T_T_V=numpify(transform.transform)
                
                # Lookup the transform from 'vrpn' to 'body'
                transform = self.tf_buffer.lookup_transform( 'vrpn','body',rospy.Time(0), rospy.Duration(1.0))
                                # Create a pose message and set its header frame ID as 'body'
                self.T_B_V=numpify(transform.transform)
                
                # Lookup the transform from 'vrpn' to 'body'
                transform = self.tf_buffer.lookup_transform( 'body','target',rospy.Time(0), rospy.Duration(1.0))
                                # Create a pose message and set its header frame ID as 'body'
                self.T_T_B=numpify(transform.transform)
                
               
                
                self.rate.sleep()
                
            except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
                rospy.logwarn("Failed to lookup transform!")
                
if __name__ == '__main__':
    node = ActiveSlotNode()
    
